#!/usr/bin/env python

#Pot rotation code
#Adam Pierce
#Must have serial node to read from ADC

#creating defines for all the counts for pots
PI = 3.141592654
XI1 =82
XF1 =261
XI2 =621
XF2 =292
XI3 =330
XF3 =667
XI4 =670
XF4 =336
XILS = 84
XFLS = 205
ALPHA1 =54
PHI1 =35
ALPHA2= 48
PHI2 =28
ALPHA3= 48
PHI3 =20
ALPHA4= 52
PHI4 =21
ALPHALS=140
PHILS=140

import roslib; roslib.load_manifest('ares_main')
import rospy
from ares_main.msg import rot_msgs
from ares_main.msg import sonar_msgs
from ares_main.msg import rot_LS_msg

# SERIAL CODE START YAYAYAYAYAY
import time

def setpin(pin, out):
  if not(out == '0' or out == '1'):
    rospy.loginfo("attempted output was: " + out)
    return
  else:
    value = open('/sys/class/gpio/gpio' + str(pin) + '/value', 'w')
    value.write(out)
    value.close()
    return
    
def readpin(pin):
  value = open('/sys/class/gpio/gpio' + str(pin) + '/value', 'r')
  val = value.read()
  #rospy.loginfo("read value: " + str(True if val.strip() == '1' else False))
  value.close()
  return True if val.strip() == '1' else False

# read SPI data from MCP3008 chip, 8 possible adc's (0 thru 7)
def readadc(portpin, clockpin, mosipin, misopin, cspin):
  if ((portpin > 7) or (portpin < 0)):
    return -1
  setpin(cspin, '1')
 
  setpin(clockpin, '0') # start clock low
  setpin(cspin, '0') # bring CS low
   
  commandout = portpin
  commandout |= 0x18 # start bit + single-ended bit
  commandout <<= 3 # we only need to send 5 bits here
  for i in range(5):
    if (commandout & 0x80):
      setpin(mosipin, '1')
    else:
      setpin(mosipin, '0')
    commandout <<= 1
    setpin(clockpin, '1')
    setpin(clockpin, '0')
   
  adcout = 0
  # read in one empty bit, one null bit and 10 ADC bits
  for i in range(12):
    setpin(clockpin, '1')
    setpin(clockpin, '0')
    adcout <<= 1
    if (readpin(misopin)):
      adcout |= 0x1
   
  setpin(cspin, '1')
  #rospy.loginfo(str(adcout))
  adcout >>= 1 # first bit is 'null' so drop it
  return adcout
  

#publishing from the all the wheel modules.  Enjoy!

# change these as desired - they're the pins connected from the
# SPI port on the ADC to the Pandaboard
SPICLK0 = 59 #pin 28 on J6
SPIMISO0 = 56 #pin 26 on J6
SPIMOSI0 = 35 #pin 24 on J6
SPICS0 = 34 #pin 22 on J6

SPICLK1 = 51 #pin 27 on J6
SPIMISO1 = 50 #pin 25 on J6
SPIMOSI1 = 55 #pin 23 on J6
SPICS1 = 54 #pin 21 on J6

# 10k trim pot connected to adc #0
wheel3Pot = 2
wheel4Pot = 3
sonarUpPotB= 0
sonarDownPotB = 1

sonarUpPotF = 0
sonarDownPotF = 1
wheel1Pot = 2
LSPot = 3
wheel2Pot = 4

rospy.init_node('sensors')

pub_rot = rospy.Publisher("rotation_data/", rot_msgs)
pub_sonar = rospy.Publisher("sonar_data/", sonar_msgs)
pub_LS = rospy.Publisher("LS_data/", rot_LS_msg)

msg_rot = rot_msgs()
msg_sonar = sonar_msgs()
msg_LS = rot_LS_msg()
  
wheel3 = readadc(wheel3Pot, SPICLK0, SPIMOSI0, SPIMISO0, SPICS0)
wheel4 = readadc(wheel4Pot, SPICLK0, SPIMOSI0, SPIMISO0, SPICS0)
sonarUpB = readadc(sonarUpPotB, SPICLK0, SPIMOSI0, SPIMISO0, SPICS0)
sonarDownB = readadc(sonarDownPotB, SPICLK0, SPIMOSI0, SPIMISO0, SPICS0)

sonarUpF = readadc(sonarUpPotF, SPICLK1, SPIMOSI1, SPIMISO1, SPICS1)
sonarDownF = readadc(sonarDownPotF, SPICLK1, SPIMOSI1, SPIMISO1, SPICS1)
wheel1 = readadc(wheel1Pot, SPICLK1, SPIMOSI1, SPIMISO1, SPICS1)
wheel2 = readadc(wheel2Pot, SPICLK1, SPIMOSI1, SPIMISO1, SPICS1)
LS = readadc(LSPot, SPICLK1, SPIMOSI1, SPIMISO1, SPICS1)

M1 = wheel1
M2 = wheel2
M3 = wheel3
M4 = wheel4
MLS = LS

while not rospy.is_shutdown():
  # read the analog pin
  wheel3 = readadc(wheel3Pot, SPICLK0, SPIMOSI0, SPIMISO0, SPICS0)
  wheel4 = readadc(wheel4Pot, SPICLK0, SPIMOSI0, SPIMISO0, SPICS0)
  sonarUpB = readadc(sonarUpPotB, SPICLK0, SPIMOSI0, SPIMISO0, SPICS0)
  sonarDownB = readadc(sonarDownPotB, SPICLK0, SPIMOSI0, SPIMISO0, SPICS0)
  
  sonarUpF = readadc(sonarUpPotF, SPICLK1, SPIMOSI1, SPIMISO1, SPICS1)
  sonarDownF = readadc(sonarDownPotF, SPICLK1, SPIMOSI1, SPIMISO1, SPICS1)
  wheel1 = readadc(wheel1Pot, SPICLK1, SPIMOSI1, SPIMISO1, SPICS1)
  wheel2 = readadc(wheel2Pot, SPICLK1, SPIMOSI1, SPIMISO1, SPICS1)
  LS = readadc(LSPot, SPICLK1, SPIMOSI1, SPIMISO1, SPICS1)

  rospy.loginfo("wheel3: " + str(wheel3) + ", wheel4: " + str(wheel4) + ", sonarUpB: "+ str(sonarUpB) + ", sonarDownB: " + str(sonarDownB))

  rospy.loginfo("wheel1: " + str(wheel1) + ", wheel2: " + str(wheel2) + ", LS: " + str(LS) + ", sonarUpF: " + str(sonarUpF) + ", sonarDownF: " + str(sonarDownF))

  msg_rot.rot1 = (360.0 - PHI1 - ALPHA1) / (XF1 - XI1) * (M1 - wheel1)
  msg_rot.rot2 = (360.0 - PHI2 - ALPHA2) / (XF2 - XI2) * (M2 - wheel2)
  msg_rot.rot3 = (360.0 - PHI3 - ALPHA3) / (XF3 - XI3) * (M3 - wheel3)
  msg_rot.rot4 = (360.0 - PHI4 - ALPHA4) / (XF4 - XI4) * (M4 - wheel4)
  msg_LS.rotLS = (360.0 - PHILS - ALPHALS) / (XFLS - XILS) * (LS - MLS)
  msg_sonar.sonarUpFront = (1000 / 9.8) * sonarUpF * 5 / 1024
  msg_sonar.sonarDownFront = (1000 / 9.8) * sonarDownF * 5 / 1024
  msg_sonar.sonarUpBack = (1000 / 9.8) * sonarUpB * 5 / 1024
  msg_sonar.sonarDownBack = (1000 / 9.8) * sonarDownB * 5 / 1024
  
  
  pub_rot.publish(msg_rot)
  pub_LS.publish(msg_LS)
  pub_sonar.publish(msg_sonar)
  
  # hang out and do nothing for a 100ms
  time.sleep(0.01)
  #THIS IS WERE SER
